import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

import EDU.gatech.cc.is.util.Vec2;


public class WMFormation {
	
	public static class Teammate{
		public Vec2D pos = null;
		public int id = -1;
		public double heading=0;
		public Vec2D vectorToLeader = null;
		public Teammate(Vec2D p, int i, double h){
			pos=p; id=i; heading=h;
		}
		public Teammate(Vec2D p, int i, double h, Vec2D pos2leader){
			pos=p; id=i; heading=h; 
			vectorToLeader = pos2leader;
		}
		public Vec2D.Pose pose(){ return new Vec2D.Pose(pos, heading); }
	}
	
	static class Private{
		
		//-------------- SHARED DATA ----------------
		private boolean seeLeader = false;
		private boolean iAmLeader = false;
		private boolean seeAnchor = false;
	
		//------------- PRIVATE DATA ----------------
		public int myId = 0;
		public Vec2 goal = null;
		public Vec2 obstacle = null;
		private Teammate[] teammates = null;
		
		public Vec2D goalPosFromLeader = null;
		public int anchorId = 0;
		
		public double dist_to_leader=0;
		public double dist_to_anchor=0;
		public double dist_to_leader_real=0;
		public double dist_to_goal_real=0;
		public double dist_to_goal=0;
		
		
		//-------------- SETTERS --------------------
		synchronized public void setSeeLeader(boolean value){
			seeLeader = value;
			if(team!=null) team.signal_seeLeader(myId, value);
		}
		synchronized public void setIAmLeader(boolean value){
			iAmLeader = value;
			if(team!=null) team.signal_iAmLeader(myId, value);
		}
		synchronized public void setSeeAnchor(boolean value){
			seeAnchor = value;
			if(team!=null) team.signal_seeAnchor(myId, value);
		}
		
		public void setGoal(Vec2 g){ goal = g; }
		public void setObstacle(Vec2 o){ obstacle = o; }
		public void setAnchorId(int id){ anchorId = id; }
		public void setGoalPosition(Vec2D v){ goalPosFromLeader = v; }
		public void setTeammates(Teammate[] tm){ teammates = tm; }
		public void setTeammatesByIndex(int i, Teammate tm){ teammates[i] = tm; }
		public void setDistanceToLeader(double d){ dist_to_leader = d; }
		public void setDistanceToAnchor(double d){ dist_to_anchor = d; }
		public void setRealDistanceToLeader(double d){ dist_to_leader_real = d; }
		public void setDistanceToGoal(double d){ dist_to_goal = d; }
		public void setRealDistanceToGoal(double d){ dist_to_goal_real = d; }
		
		//------------- GETTERS ---------------------
		synchronized public boolean getSeeLeader(){ return seeLeader; }
		synchronized public boolean getSeeAnchor(){ return seeAnchor; }
		synchronized public boolean getIAmLeader(){	return iAmLeader; }

		public int getMyId(){ return myId; }
		public Vec2 getGoal(){ return goal; }
		public Vec2 getObstacle(){ return obstacle; }
		public Vec2D getGoalVectorToLeader(){ return goalPosFromLeader; }
		public int getAnchorId(){ return anchorId; }
		public Map<Integer, Teammate> getTeammates(){
			HashMap<Integer, Teammate> map = new HashMap<Integer, WMFormation.Teammate>();
			for(Teammate t: teammates) map.put(t.id, t);
			return map;
		}
		public Teammate getTeammatesByIndex(int i){
			return teammates[i];
		}
		
		public double getRealDistanceToLeader(){ return dist_to_leader_real; }
		public double getDistanceToLeader(){ return dist_to_leader; }
		public double getDistanceToAnchor(){ return dist_to_anchor; }
		public double getRealDistanceToGoal(){ return dist_to_goal_real; }
		public double getDistanceToGoal(){ return dist_to_goal; }
		
		
		
		public Private(int id, Team team){
			this.team = team;
			this.myId = id;
			if(team!=null) team.signal_addToTeam(myId);
		}
		Team team;
	}
	
	static class Team{
		
		public HashSet<Integer> setTeam = new HashSet<Integer>();
		public HashSet<Integer> setSeeLeader = new HashSet<Integer>();
		public HashSet<Integer> setSeeAnchor = new HashSet<Integer>();
		public int leaderId = 0;
		
		//------------ SIGNALS --------------------
		synchronized public void signal_addToTeam(int senderId){
			setTeam.add(senderId);
		}
		synchronized public void signal_seeLeader(int senderId, boolean value){
			if(value) setSeeLeader.add(senderId);
			else if( setSeeLeader.contains(senderId) ) setSeeLeader.remove(senderId);
		}
		synchronized public void signal_seeAnchor(int senderId, boolean value){
			if(value) setSeeAnchor.add(senderId);
			else if( setSeeAnchor.contains(senderId) ) setSeeAnchor.remove(senderId);
		}
		synchronized public void signal_iAmLeader(int senderId, boolean value){
			if(value) leaderId = senderId;
			if(!value && leaderId == senderId) leaderId = -1;
		}
		
		//------------ GETTERS --------------------
		synchronized public Set<Integer> getSetTeam(){ return new HashSet<Integer>(setTeam); }
		synchronized public Set<Integer> getSetSeeLeader(){ return new HashSet<Integer>(setSeeLeader); }
		synchronized public Set<Integer> getSetSeeAnchor(){ return new HashSet<Integer>(setSeeAnchor); }
		synchronized public int getLeaderId(){ return leaderId;	}
		
		public Team(Team steam){ superTeam = steam; }
		Team superTeam;
	}
	
	public static class Knowladge{
		public static class RobotIdentifier{
			public int id;
			public Vec2D position;
			public double heading;
			public Vec2D vectorToLeader;
			public RobotIdentifier(int i, Vec2D p, double h, Vec2D vtl){ id=i; position=p; heading = h; vectorToLeader=vtl; }
			public Integer getId(){ return new Integer(id); } 
		}
		private Map<Integer, RobotIdentifier> robotIdentifier = new HashMap<Integer, RobotIdentifier>();
		synchronized void update(RobotIdentifier ri){
			if(robotIdentifier.containsKey(ri.getId())) robotIdentifier.remove(ri.getId());
			robotIdentifier.put(ri.getId(), ri);
		}
		synchronized ArrayList<RobotIdentifier> getTeammates(){
			ArrayList<RobotIdentifier> arr = new ArrayList<RobotIdentifier>();
			arr.addAll(robotIdentifier.values());
			return arr;
		}
	}
	static public Knowladge knowladge = new Knowladge(); 
	
}
